#ifndef FILTER_H_
#define FILTER_H_

#include <Eigen/Dense>

class EKF {
public:
    EKF(const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const Eigen::MatrixXd& F, const Eigen::MatrixXd& H) {
        x_ = x0;
        P_ = P0;
        Q_ = Q;
        R_ = R;
        F_ = F;
        H_ = H;
    }

    void predict(const Eigen::VectorXd& u) {
        x_ = F_ * x_ + u;
        P_ = F_ * P_ * F_.transpose() + Q_;
    }

    void update(const Eigen::VectorXd& z) {
        Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
        Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse();
        Eigen::VectorXd y = z - H_ * x_;
        x_ = x_ + K * y;
        P_ = (Eigen::MatrixXd::Identity(P_.rows(), P_.cols()) - K * H_) * P_;
    }

    Eigen::VectorXd getState() const {
        return x_;
    }

    Eigen::MatrixXd getCovariance() const {
        return P_;
    }

private:
    Eigen::VectorXd x_; // 状态向量
    Eigen::MatrixXd P_; // 状态协方差矩阵
    Eigen::MatrixXd Q_; // 过程噪声协方差矩阵
    Eigen::MatrixXd R_; // 观测噪声协方差矩阵
    Eigen::MatrixXd F_; // 状态转移矩阵
    Eigen::MatrixXd H_; // 观测矩阵
};


#endif